clear,clc
%自己写的第二个代码，即要求实现，遇到死角之后改出来，按原路返回
%% 1初始化地图和起始点和目标点
load c.mat                                             %导入准备好的地图，从简单的开始
map = c;
start_x = 13;  start_y = 10;  end_x = 2;  end_y = 10;  %设置起始点和目标点坐标
%start_x = 21;  start_y = 2;  end_x = 2;  end_y = 21;  %设置起始点和目标点坐标
map(start_x,start_y)=0.5;   map(end_x,end_y)=0.51;
Drawamap(start_x,start_y,end_x,end_y,map);               %根据起始点、目标点和地图即可绘制初始地图
%% 2从起始点开始向四周四个方向进行扩展探索，并进行综合评价函数Fn的排序，若Fn相同，则选择启发函数最小的进行一步行动
sport = [0,1;0,-1;-1,0;1,0];                           %四种运动方式：上下左右
nowlocation = [start_x, start_y];                      %确定当前位置
explore=[];                                            %初始化探索矩阵
path = [];                                             %路径矩阵
pic_num = 1;
barrier = map;
while 1
    neighbor = zeros(4,2);                                         %设立一个邻居集合，每走一步将其重置一次
    where = [];                                            %设置一个flag，相当于对障碍物标记
    true_neighbor = zeros(4,4);                            %真实邻居矩阵，即在上一步骤中找到的邻居排除了障碍物
    worth = []; worth1 = []; worth2 = []; worth3  = [];                                       %设置每步的综合评价值

    %探索路径，排除障碍物
    for i=1:4                                              %从当前位置出发上下左右四个格子进行搜索，将四个位置坐标存入neighbor集合中%遍历四个邻居有没有在障碍物上的
        neighbor(i,1) = nowlocation(1,1) + sport(i,1);
        neighbor(i,2) = nowlocation(1,2) + sport(i,2);
        
        if barrier(neighbor(i,1),neighbor(i,2))==0.5          %走过的路标记为障碍物
            where = [where;i];
            continue
        end
        
        if barrier(neighbor(i,1),neighbor(i,2))~=0             %如果不在障碍物上，则转移到真实邻居集合中，同时计算g和h
            true_neighbor(i,1) = neighbor(i,1);            %真实邻居的X坐标
            true_neighbor(i,2) = neighbor(i,2);            %真实邻居的Y坐标
            true_neighbor(i,3) = abs(start_x - neighbor(i,1)) +  abs(start_y - neighbor(i,2));        %真实邻居的g即到达此位置的代价
            true_neighbor(i,4) = abs(end_x - neighbor(i,1))   +  abs(end_y - neighbor(i,2));
            %true_neighbor(i,4) = sqrt((abs(end_x - neighbor(i,1)))^2   +  (abs(end_y - neighbor(i,2)))^2);          %真实邻居的启发函数h，到目标点的预估值
        elseif barrier(neighbor(i,1),neighbor(i,2))==0         %如果在障碍物上
            where = [where;i];                             %标记障碍物在四个方向上的哪一个
        end
    end
    where(end:-1:1)=where;                                 %障碍物标记倒序，这样好删除
    for i=1:size(where)                                    %遍历障碍物个数
        true_neighbor(where(i),:)=[];                      %在真实邻居中删除障碍物
    end

    n=size(explore,1);                                     %获取现在已经探索的路径有多少
    for i=1:size(true_neighbor,1)                          %将此步中获得的探索值写入探索路径矩阵，用来画图
        explore(n+i,:)=true_neighbor(i,:);
    end

    for i=1:size(true_neighbor,1)                          %为当前可选择的路进行排序
        worth(i,:) = [i, true_neighbor(i,3)+true_neighbor(i,4) , true_neighbor(i,4)];
    end
    
    %此处是第二个代码的核心修改
    if size(worth,1)==0
        %将此步骤移除路径矩阵，并将此处认定为障碍物，返回上一步，重新规划；
        barrier(path(size(path,1),:))=0;                         %将死角路径标记为障碍物
        path(size(path,1),:)=[];                                 %将次步骤从路径集合中删去
        n=size(path,1);                                     
        nowlocation(1,1) = path(n,1);                            %退回到上一步路径
        nowlocation(1,2) = path(n,2);
    else
        worth1 = sortrows(worth,2);  %此处仍需再排一次。
        worth2(1,:)=worth1(1,:);
        for i=1:size(worth1)-1
            if worth1(i+1,2)~=worth1(i,2)
                break
            else
                worth2(i+1,:)=worth1(i+1,:);
            end
        end
        worth3=sortrows(worth2,3);
                
            
        %更新当前位置坐标
        nowlocation(1,1) = true_neighbor(worth3(1),1);
        nowlocation(1,2) = true_neighbor(worth3(1),2);
        barrier(nowlocation(1,1),nowlocation(1,2))=0.5;          %并将其进行标记，避免返回原来走过的路径

        n=size(path,1);                                          %获取现在已经走过的路径有多少，为后面的撞墙返回提供索引
        path(n+1,:)=[nowlocation(1,1),nowlocation(1,2)];
    end
    
    map = barrier;
    
    Drawapath(start_x,start_y,end_x,end_y,map,explore,path)  %画图，画出目前探索的地图和走过的路径
    makeaGIF(pic_num)                                        %制作动图
    pic_num = pic_num + 1;                                   %动图帧数
    if nowlocation(1,1)==end_x && nowlocation(1,2)==end_y    %判断是否到达终点，若到达则跳出循环结束
        break
    end
end





